#include <iostream>
#include <opencv2/core.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/imgproc.hpp>

cv::Mat my_filter_2d(const cv::Mat& src, const cv::Mat& kernel, cv::Point anchor, double delta, int borderType) {
    CV_Assert(src.type() == CV_8UC1);
    CV_Assert(kernel.type() == CV_32SC1);
    CV_Assert(kernel.rows % 2 == 1 && kernel.cols % 2 == 1);
    CV_Assert(anchor.x == -1 || (anchor.x >= 0 && anchor.x < kernel.cols));
    CV_Assert(anchor.y == -1 || (anchor.y >= 0 && anchor.y < kernel.rows));
    
    cv::Mat dst(src.size(), src.type());
    int top = anchor.y == -1 ? kernel.rows / 2 : anchor.y;
    int bottom = anchor.y == -1 ? kernel.rows / 2 : kernel.rows - 1 - anchor.y;
    int left = anchor.x == -1 ? kernel.cols / 2 : anchor.x;
    int right = anchor.x == -1 ? kernel.cols / 2 : kernel.cols - 1 - anchor.x;
    cv::Mat srcPadded;
    cv::copyMakeBorder(src, srcPadded, top, bottom, left, right, borderType);
    for (int i = 0; i < src.rows; i++) {
        uchar* p = dst.ptr<uchar>(i);
        for (int j = 0; j < src.cols; j++) {
            cv::Mat roi(srcPadded, cv::Rect(j, i, kernel.cols, kernel.rows));
            cv::Mat roiInt;
            roi.convertTo(roiInt, CV_32SC1);
            cv::Mat dstInt;
            cv::multiply(roiInt, kernel, dstInt);
            cv::Scalar s = cv::sum(dstInt);
            p[j] = cv::saturate_cast<uchar>(s[0] + delta);
        }
    }
    return dst;
}

int main(int argc, char **argv) {
    cv::Mat src(15, 16, CV_8UC1);
    cv::randu(src, cv::Scalar(0), cv::Scalar(255));
    
    cv::Mat dst;
    int ddepth = -1;
    cv::Mat kernel = (cv::Mat_<int>(3, 3) << 1, 2, 3, 4, 0, -4, -3, -2, -1);
    cv::Point anchor(1, 2);
    double delta = 3;
    int borderType = cv::BORDER_REFLECT101;
    cv::filter2D(src, dst, ddepth, kernel, anchor, delta, borderType);
    cv::Mat my_dst = my_filter_2d(src, kernel, anchor, delta, borderType);
    cv::Mat diff = my_dst != dst;
    std::vector<cv::Point> nonzeros;
    cv::findNonZero(diff, nonzeros);
    std::cout << "src = \n" << src << std::endl;
    std::cout << "dst = \n" << dst << std::endl;
    std::cout << "my_dst = \n" << my_dst << std::endl;
    std::cout << "nonzeros = " << nonzeros << std::endl;
    
    return EXIT_SUCCESS;
}
